#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "libi2c.h"

#define TOF_I2C_PORT_NO    1 //i2c 4 is mapped to /dev/i2c-1
#define TOF_I2C_ADDR   0x51 

void init_tof(char i2c_port, char i2c_addr)
{
	char i2c_read_buf[2] = {0};
	char i2c_write_buf[2] = {0};
	struct ar_usr_i2c st_i2c_read = {0};
	struct ar_usr_i2c st_i2c_write = {0};
	
	ar_i2c_init(i2c_port);

        //is this needed ?
#if 1
	i2c_write_buf[1] = 0x00;
	i2c_write_buf[0] = 0x80;
	st_i2c_write.ic_addr = i2c_addr;
	st_i2c_write.reg_addr = 0x00;
	st_i2c_write.date_len = 2;
	st_i2c_write.data = i2c_write_buf;
	st_i2c_write.type = AR_I2C_TYPE_8BIT_ADDR;
	st_i2c_write.port = i2c_port;
	
	ar_i2c_write_word(&st_i2c_write);

	i2c_write_buf[1] = 0xFF;
	i2c_write_buf[0] = 0xFF;
	st_i2c_write.reg_addr = 0x01;
	st_i2c_write.date_len = 2;
	st_i2c_write.data = i2c_write_buf;
	
	ar_i2c_write_word(&st_i2c_write);

	i2c_write_buf[1] = 0x00;
	i2c_write_buf[0] = 0x00;
	st_i2c_write.reg_addr = 0x02;
	st_i2c_write.date_len = 2;
	st_i2c_write.data = i2c_write_buf;
	
	ar_i2c_write_word(&st_i2c_write);

	i2c_write_buf[1] = 0x08;
	i2c_write_buf[0] = 0x09;
	st_i2c_write.reg_addr = 0x03;
	st_i2c_write.date_len = 2;
	st_i2c_write.data = i2c_write_buf;
	ar_i2c_write_word(&st_i2c_write);
	
	i2c_write_buf[1] = 0x08;
	i2c_write_buf[0] = 0x61;
	st_i2c_write.reg_addr = 0x04;
	st_i2c_write.date_len = 2;
	st_i2c_write.data = i2c_write_buf;
	ar_i2c_write_word(&st_i2c_write);
	
	i2c_write_buf[1] = 0x00;
	i2c_write_buf[0] = 0x00;
	st_i2c_write.reg_addr = 0x06;
	st_i2c_write.date_len = 2;
	st_i2c_write.data = i2c_write_buf;
	ar_i2c_write_word(&st_i2c_write);
	
	i2c_write_buf[1] = 0xFF;
	i2c_write_buf[0] = 0xFF;
	st_i2c_write.reg_addr = 0x07;
	st_i2c_write.date_len = 2;
	st_i2c_write.data = i2c_write_buf;
	ar_i2c_write_word(&st_i2c_write);

	st_i2c_read.ic_addr = i2c_addr;
	st_i2c_read.reg_addr = 0x03;
	st_i2c_read.date_len = 2;
	st_i2c_read.data = i2c_read_buf;
	st_i2c_read.type = AR_I2C_TYPE_8BIT_ADDR;
	st_i2c_read.port = i2c_port;
	
	ar_i2c_read_word(&st_i2c_read);

	if(i2c_read_buf[1] != 0x08)
	{
		printf("TOF-RD: 0x03 val %x\n", i2c_read_buf[1]);
//		return;
	}

	printf("TOF init OK.\n");
#endif
	st_i2c_write.ic_addr = i2c_addr;
	st_i2c_write.data = i2c_write_buf;
	st_i2c_write.type = AR_I2C_TYPE_8BIT_ADDR;
	st_i2c_write.port = i2c_port;
	i2c_write_buf[1] = 0x08;
	i2c_write_buf[0] = 0x08;
	st_i2c_write.reg_addr = 0x03;
	st_i2c_write.date_len = 2;
	ar_i2c_write_word(&st_i2c_write);
	printf("TOF enabled.\n");

}

unsigned short read_tof_ps(char i2c_port, char i2c_addr)
{
	char i2c_read_buf[2] = {0};
	struct ar_usr_i2c st_i2c_read = {0};

	st_i2c_read.ic_addr = i2c_addr;
	st_i2c_read.reg_addr = 0x08;
	st_i2c_read.date_len = 2;
	st_i2c_read.data = i2c_read_buf;
	st_i2c_read.type = AR_I2C_TYPE_8BIT_ADDR;
	st_i2c_read.port = i2c_port;
	
	ar_i2c_read_word(&st_i2c_read);

	return i2c_read_buf[1] << 8 | i2c_read_buf[0];
}

unsigned short read_tof_als(char i2c_port, char i2c_addr)
{
	unsigned short value = 0;
	char i2c_read_buf[2] = {0};
	struct ar_usr_i2c st_i2c_read = {0};

	st_i2c_read.ic_addr = i2c_addr;
	st_i2c_read.reg_addr = 0x09;
	st_i2c_read.date_len = 2;
	st_i2c_read.data = i2c_read_buf;
	st_i2c_read.type = AR_I2C_TYPE_8BIT_ADDR;
	st_i2c_read.port = i2c_port;
	
	ar_i2c_read_word(&st_i2c_read);

	printf("===rd buf %x %x\n", i2c_read_buf[1], i2c_read_buf[0]);
	value = (unsigned short)i2c_read_buf[1];

	return value << 8 | i2c_read_buf[0];
}

int main(int argc, char * argv[])
{	
	char i2c_port = 0, i2c_addr = 0;

	unsigned short dist_ps = 0, dist_als = 0;

	if(argc < 2)
	{
		printf("Usage: %s [i2c_port]\n", argv[0]);
		return 0;
	}

	i2c_port = atoi(argv[1]);
	i2c_addr = TOF_I2C_ADDR;

	printf("======TOF test: i2c %d addr %x======\n", i2c_port, i2c_addr);

	init_tof(i2c_port, i2c_addr);

	while(1)
	{
		dist_ps = read_tof_ps(i2c_port, i2c_addr);
		dist_als = read_tof_als(i2c_port, i2c_addr);

		printf("TOF RD: %d %d\n", dist_ps, dist_als);
		sleep(1);
	}

	return 0;
}
